#pragma config(UART_Usage, UART1, uartVEXLCD, baudRate19200, IOPins, None, None)
#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(Sensor, in2,    gyro,           sensorGyro)
#pragma config(Sensor, in3,    armMeasure,     sensorPotentiometer)
#pragma config(Sensor, in4,    armSetPot,      sensorPotentiometer)
#pragma config(Sensor, dgtl2,  ,               sensorDigitalOut)
#pragma config(Sensor, dgtl7,  brake,          sensorDigitalOut)
#pragma config(Sensor, dgtl8,  trayTilt,       sensorDigitalOut)
#pragma config(Sensor, dgtl9,  limitSwitch,    sensorTouch)
#pragma config(Motor,  port1,           backleft,      tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port2,           frontleft,     tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port3,           armbottomleft, tmotorVex393, openLoop)
#pragma config(Motor,  port4,           armtopleft,    tmotorVex393, openLoop)
#pragma config(Motor,  port5,           intakeleft,    tmotorVex393, openLoop)
#pragma config(Motor,  port6,           intakeright,   tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port7,           armtopright,   tmotorVex393, openLoop)
#pragma config(Motor,  port8,           armbottomright, tmotorVex393, openLoop)
#pragma config(Motor,  port9,           frontright,    tmotorVex393HighSpeed, openLoop, reversed)
#pragma config(Motor,  port10,          backright,     tmotorVex393HighSpeed, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#include "PIDController.c"
#include "Functions.c"
#include "LCD.c"

//*****Hardware Varibles*****//
int threshold=2;

//arm
int armMin=1200;
int armMax=3050;
int armScore=2400;
float armRange=armMax-armMin;
int armPower=0;
int inPower=0;
//arm PID
PIDController armPID;
bool armSetPointSet=false;
int armSetPoint=armMin;
int armResult=0;

//drive
int walk=0, strafe=0, turn=0;
//cheesy vars
float skim=0, turnGain=0.0102;//gives turn*1.3 at full walk
//gyro PID
PIDController gyroPID;
bool gyroSetPointSet=false;
bool autonOverride=true;
int gyroSetPoint=0;//tenths of a degree, CCW+
int gyroResult=0;


//*****Hardware Functions*****/
task findPIDs(){
	int delta=25;
	while(true){
		setSetpoint(armPID, armSetPoint,true);
		armResult=calculatePID(armPID,SensorValue[armMeasure],delta);

		//DISABLE GYRO 04/16
		setSetpoint(gyroPID,gyroSetPoint,true);
		//gyroResult=calculatePID(gyroPID,SensorValue[gyro],delta);
		gyroResult=0;

		wait1Msec(delta);
	}
}

int getArm(){
	return SensorValue[armMeasure];
}

int getArmSet(){
	return SensorValue[armSetPot];
}

int getGyro(){
	return SensorValue[gyro];
}

void initGyro(){
	SensorType[gyro] = sensorNone;
	wait1Msec(1000);
	SensorType[gyro] = sensorGyro;
	wait1Msec(2000);
	SensorValue[gyro]=0;
	SensorFullCount[gyro]=36000;
}

void initPIDs(){
	//Arm PID
	init(armPID);
	setPIDs(armPID,0.2,0.0,0.0);
	enable(armPID);

	//gyro PID
	init(gyroPID);
	setPIDs(gyroPID,0.4,0,0.9);
	enable(gyroPID);

	StartTask(findPIDs);
}

void testPID(float p, float i, float d){
	setPIDs(gyroPID,p,i,d);
}

void stopAllMotors(){
	motor[1]=motor[2]=motor[3]=motor[4]=motor[5]=motor[6]=motor[7]=motor[8]=motor[9]=motor[0]=0;
}

void scaleInputs(){
	//thresholds
	if(abs(walk)<threshold) walk=0;
	if(abs(strafe)<threshold) strafe=0;
	if(abs(turn)<threshold) turn=0;
	if(abs(armPower)<threshold) armPower=0;

	//Scale inputs quadratically
	walk=sign(walk)*walk*walk/127;
	strafe=sign(strafe)*strafe*strafe/127;
	turn=sign(turn)*turn*turn/127;
	armPower=sign(armPower)*armPower*armPower/127;
}

task dialAnArm(){
	while(true){
		float temp=getArmSet()*armRange;
		temp=temp/4096.0+armMin;
		armSetPoint=temp;
		motor[armtopleft] = motor[armbottomleft] = motor[armtopright] = motor[armbottomright]= armResult;
		wait1Msec(100);
	}
}//end dialAnArm




void driveMecanum(int walk_l, int strafe_l, int turn_l){
	walk=walk_l;
	strafe=strafe_l;
	turn=turn_l;

	//correct for skewed strafe with gyro
	if(turn==0 && (walk!=0 || strafe!=0) && autonOverride==false){
		if(gyroSetPointSet==false){
			gyroSetPoint=getGyro();
			gyroSetPointSet=true;
		}
		turn=-gyroResult;
	}
	else{
		gyroSetPointSet=false;
	}

	//scale inputs to maintain ratio if we have overflow
	//these variables won't round
	float w=walk,s=strafe,t=turn;
	float sum=abs(walk)+abs(strafe)+abs(turn);
	if(sum>127){
		w=127.0*w/sum;
		s=127.0*s/sum;
		t=127.0*t/sum;

		walk=w;
		strafe=s;
		turn=t;
	}

	//power each motor correctly
	motor[frontleft]=walk+strafe+turn;
	motor[frontright]=walk-strafe-turn;
	motor[backleft]=walk-strafe+turn;
	motor[backright]=walk+strafe-turn;
}//end driveMecanum



void driveCheesy(int walk_l, int strafe_l, int turn_l){
	walk=walk_l;
	strafe=strafe_l;
	turn=turn_l;
	if(walk>30){
		turn= turn*turnGain*abs(walk);
	}
	driveMecanum(walk,strafe,turn);
}

task gyroControl(){
	gyroSetPoint=getGyro();
	while(autonOverride){
		turn=-gyroResult;
		driveMecanum(walk,strafe,turn);
		wait1Msec(20);
	}
}//end gyroControl

void armMove(int armPower)
{
	//use PID
	if(armPower==0){
		if(armSetPointSet==false){
			armSetPoint=getArm();
			armSetPointSet=true;
		}
		if(getArm()<=armMin && armResult<0){
			armResult=0;
		}
		if(getArm()>=armMax && armResult>0){
			armResult=0;
		}
		motor[armtopleft] = motor[armbottomleft] = motor[armtopright] = motor[armbottomright]= -armResult;
	}
	//use controller
	else{
		armSetPointSet=false;
		motor[armtopleft] = motor[armbottomleft] = motor[armtopright] = motor[armbottomright]= armPower;
	}
}//end armMove

task armControl(){
	armSetPoint=getArm();
	while(autonOverride){
		motor[armtopleft] = motor[armbottomleft] = motor[armtopright] = motor[armbottomright]= -armResult;
		wait1Msec(20);
	}
}//end armControl

bool lastTilt=false;
void toggleTrayTilt(){
	if(lastTilt){
		SensorValue[trayTilt]=1;
	}
	else{
		SensorValue[trayTilt]=0;
	}
	lastTilt=!lastTilt;
}//end toggleArmTilt


void IntakeMove(int inPower){
	motor[intakeleft]=motor[intakeright]=inPower;
}//end IntakeMove
